#include <unistd.h>
#include <stdlib.h>
#include <math.h>
#include <stdio.h>
#include <string.h>
#include <signal.h>
#include <getopt.h>
#include <time.h>
#include <pthread.h>
#include <fcntl.h>
#include <ctype.h>
#include <cobalt/sys/cobalt.h>
#include "udd_omap_gpio.h"

#define LED1 GPIO_TO_PIN(0, 17)
#define LED2 GPIO_TO_PIN(0, 16)
#define IN GPIO_TO_PIN(3, 20)

pthread_t gpio_thread;

void cleanup_upon_sig(int sig __attribute__((unused)))
{
    pthread_cancel(gpio_thread);
    pthread_join(gpio_thread, NULL);
    udd_gpio_cleanup();
    exit(0);
}

void *thread(void *dummy){
    int in_value;

    gpio_direction_output(LED1, 0);
    gpio_direction_output(LED2, 0);
    gpio_direction_input(IN);

    while(1) {
        usleep(1000000);
        in_value = gpio_get(IN);
        printf("gpio %d:%d\n",IN, in_value);
        gpio_set_value(LED1, in_value);
        gpio_set_value(LED2, in_value);
    }
}

int main (int argc, char **argv)
{
    int err;
    char *progname = (char*)basename(argv[0]);
    struct sched_param param = { .sched_priority = 99 };
    pthread_attr_t thattr;

    signal(SIGINT, cleanup_upon_sig);
    signal(SIGTERM, cleanup_upon_sig);
    signal(SIGHUP, cleanup_upon_sig);
    signal(SIGALRM, cleanup_upon_sig);

    udd_gpio_init();

    // Thread attributes
    pthread_attr_init(&thattr);

    // Priority, set priority to 99
    pthread_attr_setinheritsched(&thattr, PTHREAD_EXPLICIT_SCHED);
    pthread_attr_setschedpolicy(&thattr, SCHED_FIFO);
    pthread_attr_setschedparam(&thattr, &param);

    // Create thread
    err = pthread_create(&gpio_thread, &thattr, &thread, NULL);
    if (err) {
        fprintf(stderr,"square: failed to create square thread, code %d\n",err);
        return 0;
    }

    pause();

    return 0;
}

